# 未测试
import time
import numpy as np
import unitree
import Unitree_Python_sdk
unitree_robot = Unitree_Python_sdk.Unitree_Robot_Low()

Tpi = 0
motion_time = 0
while True:
    time.sleep(0.002)
    motion_time += 1

    if motion_time >= 500:
        speed = 1 * sin(3*np.pi*Tpi/1000.0)

        unitree_robot.robot_work(motor=unitree.FL_2,
                                 angle=unitree.PosStopF,
                                 velocity=speed,
                                 torque_output=0.0,
                                 position_stiffness=0,
                                 velocity_stiffness=4)
        Tpi += 1

    if motion_time > 10:
        ##
